#include "TBMat4x4.h"
#include "TBQuat.h"

namespace tb
{

	TB_INLINE Mat3x3::Mat3x3() 
	{
		Identity();
	}

	TB_INLINE Mat3x3::Mat3x3(
		float a_X1, float a_Y1, float a_WX,
		float a_X2, float a_Y2, float a_WY,
		float a_TX, float a_TY, float a_CZ
	)
	{
		values[X1] = a_X1;
		values[Y1] = a_Y1;
		values[WX] = a_WX;

		values[X2] = a_X2;
		values[Y2] = a_Y2;
		values[WY] = a_WY;

		values[TX] = a_TX;
		values[TY] = a_TY;
		values[CZ] = a_CZ;
	}

	TB_INLINE Mat3x3::Mat3x3(const Mat3x3& a_Other)
	{
		values[X1] = a_Other.values[X1];
		values[Y1] = a_Other.values[Y1];
		values[WX] = a_Other.values[WX];

		values[X2] = a_Other.values[X2];
		values[Y2] = a_Other.values[Y2];
		values[WY] = a_Other.values[WY];

		values[TX] = a_Other.values[TX];
		values[TY] = a_Other.values[TY];
		values[CZ] = a_Other.values[CZ];
	}

	TB_INLINE Mat3x3::~Mat3x3()
	{

	}

	// ================================
	// Conversion
	// ================================

	TB_INLINE Mat3x3& Mat3x3::FromEulerAngle(float a_Degrees)
	{
		values[WX] = values[WY] = 
		values[TX] = values[TY] =
		values[CZ] = 0.f;

		float rads = Math::DegToRad(a_Degrees);
		const float cosAngle = Math::Cos(rads);
		const float sinAngle = Math::Sin(rads);

		values[X1] = -sinAngle; values[Y1] =  cosAngle;
		values[X2] =  cosAngle; values[Y2] =  sinAngle;

		return *this;
	}

	TB_INLINE Mat3x3& Mat3x3::FromOrthographic(float a_Left, float a_Right, float a_Top, float a_Bottom)
	{
		values[X1] = 2.f / (a_Right - a_Left);
		values[Y1] = 0.f;
		values[WX] = 0.f;

		values[X2] = 0.f;
		values[Y2] = 2.f / (a_Top - a_Bottom);
		values[WY] = 0.f;

		values[TX] = -(a_Right + a_Left) / (a_Right - a_Left);
		values[TY] = -(a_Top + a_Bottom) / (a_Top - a_Bottom);
		values[CZ] = 1.f;

		return *this;
	}

	TB_INLINE Mat3x3& Mat3x3::Decompose(Vec2& a_Translation, float& a_Rotation, Vec2& a_Scale)
	{
		a_Translation.x = values[TX];
		a_Translation.y = values[TY];

		a_Scale.x = Math::Sqrt((values[X1] * values[X1]) + (values[Y1] * values[Y1]));
		a_Scale.y = Math::Sqrt((values[X2] * values[X2]) + (values[Y2] * values[Y2]));

		a_Rotation = Math::RadToDeg(Math::ArcTan2(values[X2] / a_Scale.y, values[Y2] / a_Scale.y));

		return *this;
	}

	// ================================
	// Assignment & Access
	// ================================

	TB_INLINE Mat3x3& Mat3x3::operator = (const float* a_Values)
	{
		values[X1] = a_Values[X1];
		values[Y1] = a_Values[Y1];
		values[WX] = a_Values[WX];

		values[X2] = a_Values[X2];
		values[Y2] = a_Values[Y2];
		values[WY] = a_Values[WY];

		values[TX] = a_Values[TX];
		values[TY] = a_Values[TY];
		values[CZ] = a_Values[CZ];

		return *this;
	}

	TB_INLINE Mat3x3& Mat3x3::operator = (const Mat3x3& a_Other)
	{
		values[X1] = a_Other.values[X1];
		values[Y1] = a_Other.values[Y1];
		values[WX] = a_Other.values[WX];

		values[X2] = a_Other.values[X2];
		values[Y2] = a_Other.values[Y2];
		values[WY] = a_Other.values[WY];

		values[TX] = a_Other.values[TX];
		values[TY] = a_Other.values[TY];
		values[CZ] = a_Other.values[CZ];

		return *this;
	}

	TB_INLINE Mat3x3& Mat3x3::operator = (const Mat4x4& a_Other)
	{
		values[X1] = a_Other.values[Mat4x4::X1];
		values[Y1] = a_Other.values[Mat4x4::Y1];
		values[WX] = a_Other.values[Mat4x4::WX];

		values[X2] = a_Other.values[Mat4x4::X2];
		values[Y2] = a_Other.values[Mat4x4::Y2];
		values[WY] = a_Other.values[Mat4x4::WY];

		values[TX] = a_Other.values[Mat4x4::TX];
		values[TY] = a_Other.values[Mat4x4::TY];
		values[CZ] = a_Other.values[Mat4x4::CZ];

		return *this;
	}

	TB_INLINE float& Mat3x3::operator [] (unsigned int a_Index)
	{
		return values[a_Index];
	}

	TB_INLINE Mat3x3::operator float* ()
	{
		return values;
	}

	TB_INLINE Vec3 Mat3x3::GetColumn(int a_Column)
	{
		return Vec3(
			values[a_Column * 3 + 0],
			values[a_Column * 3 + 1],
			values[a_Column * 3 + 2]
		);
	}

	TB_INLINE Vec3 Mat3x3::GetRow(int a_Row)
	{
		return Vec3(
			values[0 + a_Row],
			values[3 + a_Row],
			values[6 + a_Row]
		);
	}


	// ================================
	// Operations
	// ================================

	TB_INLINE Vec2 Mat3x3::operator * (const Vec2& a_Vector) const
	{
		Vec2 result;

		float x = (a_Vector.x * values[X1]) + (a_Vector.y * values[X2]) + values[TX];
		float y = (a_Vector.x * values[Y1]) + (a_Vector.y * values[Y2]) + values[TY];
		float z = (a_Vector.x * values[WX]) + (a_Vector.y * values[WY]) + values[CZ];

		result.x = x / z;
		result.y = y / z;

		return result;
	}

	TB_INLINE Vec3 Mat3x3::operator * (const Vec3& a_Vector) const
	{
		return Vec3(
			(a_Vector.x * values[X1]) + (a_Vector.y * values[X2]) + values[TX],
			(a_Vector.x * values[Y1]) + (a_Vector.y * values[Y2]) + values[TY],
			(a_Vector.x * values[WX]) + (a_Vector.y * values[WY]) + values[CZ]
		);
	}

	TB_INLINE Mat3x3 Mat3x3::operator + (const Mat3x3& a_Other) const
	{
		Mat3x3 result;

		result.values[X1] = values[X1] + a_Other.values[X1];
		result.values[Y1] = values[Y1] + a_Other.values[Y1];
		result.values[WX] = values[WX] + a_Other.values[WX];

		result.values[X2] = values[X2] + a_Other.values[X2];
		result.values[Y2] = values[Y2] + a_Other.values[Y2];
		result.values[WY] = values[WY] + a_Other.values[WY];

		result.values[TX] = values[TX] + a_Other.values[TX];
		result.values[TY] = values[TY] + a_Other.values[TY];
		result.values[CZ] = values[CZ] + a_Other.values[CZ];

		return result;
	}

	TB_INLINE Mat3x3& Mat3x3::operator += (const Mat3x3& a_Other)
	{
		values[X1] += a_Other.values[X1];
		values[Y1] += a_Other.values[Y1];
		values[WX] += a_Other.values[WX];

		values[X2] += a_Other.values[X2];
		values[Y2] += a_Other.values[Y2];
		values[WY] += a_Other.values[WY];

		values[TX] += a_Other.values[TX];
		values[TY] += a_Other.values[TY];
		values[CZ] += a_Other.values[CZ];

		return *this;
	}

	TB_INLINE Mat3x3 Mat3x3::operator * (const Mat3x3& a_Other) const
	{
		float a0, a1, a2;

		Mat3x3 result;

		a0 = a_Other.values[X1];
		a1 = a_Other.values[Y1];
		a2 = a_Other.values[WX];

		result.values[X1] = (a0 * values[X1]) + (a1 * values[X2]) + (a2 * values[TX]);
		result.values[Y1] = (a0 * values[Y1]) + (a1 * values[Y2]) + (a2 * values[TY]);
		result.values[WX] = (a0 * values[WX]) + (a1 * values[WY]) + (a2 * values[CZ]);

		a0 = a_Other.values[X2];
		a1 = a_Other.values[Y2];
		a2 = a_Other.values[WY];

		result.values[X2] = (a0 * values[X1]) + (a1 * values[X2]) + (a2 * values[TX]);
		result.values[Y2] = (a0 * values[Y1]) + (a1 * values[Y2]) + (a2 * values[TY]);
		result.values[WY] = (a0 * values[WX]) + (a1 * values[WY]) + (a2 * values[CZ]);

		a0 = a_Other.values[TX];
		a1 = a_Other.values[TY];
		a2 = a_Other.values[CZ];

		result.values[TX] = (a0 * values[X1]) + (a1 * values[X2]) + (a2 * values[TX]);
		result.values[TY] = (a0 * values[Y1]) + (a1 * values[Y2]) + (a2 * values[TY]);
		result.values[CZ] = (a0 * values[WX]) + (a1 * values[WY]) + (a2 * values[CZ]);

		return result;
	}

	TB_INLINE Mat3x3& Mat3x3::operator *= (const Mat3x3& a_Other)
	{
		float a0, a1, a2;

		Mat3x3 temp = *this;

		a0 = a_Other.values[X1];
		a1 = a_Other.values[Y1];
		a2 = a_Other.values[WX];

		values[X1] = (a0 * temp.values[X1]) + (a1 * temp.values[X2]) + (a2 * temp.values[TX]);
		values[Y1] = (a0 * temp.values[Y1]) + (a1 * temp.values[Y2]) + (a2 * temp.values[TY]);
		values[WX] = (a0 * temp.values[WX]) + (a1 * temp.values[WY]) + (a2 * temp.values[CZ]);

		a0 = a_Other.values[X2];
		a1 = a_Other.values[Y2];
		a2 = a_Other.values[WY];

		values[X2] = (a0 * temp.values[X1]) + (a1 * temp.values[X2]) + (a2 * temp.values[TX]);
		values[Y2] = (a0 * temp.values[Y1]) + (a1 * temp.values[Y2]) + (a2 * temp.values[TY]);
		values[WY] = (a0 * temp.values[WX]) + (a1 * temp.values[WY]) + (a2 * temp.values[CZ]);

		a0 = a_Other.values[TX];
		a1 = a_Other.values[TY];
		a2 = a_Other.values[CZ];

		values[TX] = (a0 * temp.values[X1]) + (a1 * temp.values[X2]) + (a2 * temp.values[TX]);
		values[TY] = (a0 * temp.values[Y1]) + (a1 * temp.values[Y2]) + (a2 * temp.values[TY]);
		values[CZ] = (a0 * temp.values[WX]) + (a1 * temp.values[WY]) + (a2 * temp.values[CZ]);

		return *this;
	}

	TB_INLINE Mat3x3 Mat3x3::operator * (float a_Value) const
	{
		return Mat3x3(
			values[X1] * a_Value, values[Y1] * a_Value, values[WX] * a_Value, 
			values[X2] * a_Value, values[Y2] * a_Value, values[WY] * a_Value, 
			values[TX] * a_Value, values[TY] * a_Value, values[CZ] * a_Value 
		);
	}

	TB_INLINE Mat3x3& Mat3x3::operator *= (float a_Value)
	{
		values[X1] *= a_Value;
		values[Y1] *= a_Value;
		values[WX] *= a_Value;

		values[X2] *= a_Value;
		values[Y2] *= a_Value;
		values[WX] *= a_Value;

		values[TX] *= a_Value;
		values[TY] *= a_Value;
		values[CZ] *= a_Value;

		return *this;
	}

	TB_INLINE Mat3x3 Mat3x3::operator / (float a_Value) const
	{
		return Mat3x3(
			values[X1] / a_Value, values[Y1] / a_Value, values[WX] / a_Value, 
			values[X2] / a_Value, values[Y2] / a_Value, values[WY] / a_Value, 
			values[TX] / a_Value, values[TY] / a_Value, values[CZ] / a_Value
		);
	}

	TB_INLINE Mat3x3& Mat3x3::operator /= (float a_Value)
	{
		values[X1] /= a_Value;
		values[Y1] /= a_Value;
		values[WX] /= a_Value;

		values[X2] /= a_Value;
		values[Y2] /= a_Value;
		values[WX] /= a_Value;

		values[TX] /= a_Value;
		values[TY] /= a_Value;
		values[CZ] /= a_Value;

		return *this;
	}

	// ================================
	// Comparisons
	// ================================

	TB_INLINE bool Mat3x3::operator == (const Mat3x3& a_Other) const
	{
		for (int i = 0; i < 16; i++)
		{
			if (values[i] != a_Other.values[i]) { return false; }
		}

		return true;
	}

	TB_INLINE bool Mat3x3::operator == (float* a_Values) const
	{
		for (int i = 0; i < 9; i++)
		{
			if (values[i] != a_Values[i]) 
			{ 
				return false; 
			}
		}

		return true;
	}

	TB_INLINE bool Mat3x3::operator != (const Mat3x3& a_Other) const
	{
		for (int i = 0; i < 9; i++)
		{
			if (values[i] != a_Other.values[i]) { return true; }
		}

		return false;
	}

	TB_INLINE bool Mat3x3::Compare(const Mat3x3& a_Other, float a_Delta /*= 1e-6f*/) const
	{
		for (int i = 0; i < 9; i++)
		{
			if ((values[i] - a_Other.values[i]) * 2 > a_Delta)
			{
				return false; 
			}
		}

		return true;
	}

	TB_INLINE bool Mat3x3::Compare(float* a_Values, float a_Delta /*= 1e-6f*/) const
	{
		for (int i = 0; i < 9; i++)
		{
			if ((values[i] - a_Values[i]) * 2 > a_Delta)
			{
				return false; 
			}
		}

		return true;
	}

	TB_INLINE bool Mat3x3::IsIdentity(float a_Delta /*= 1e-6f*/) const
	{
		if (values[X1] - 1.f < -a_Delta || values[X1] - 1.f > a_Delta) { return false; }
		if (values[Y1] < -a_Delta || values[Y1] > a_Delta) { return false; }
		if (values[WX] < -a_Delta || values[WX] > a_Delta) { return false; }

		if (values[X2] < -a_Delta || values[X2] > a_Delta) { return false; }
		if (values[Y2] - 1.f < -a_Delta || values[Y2] - 1.f > a_Delta) { return false; }
		if (values[WY] < -a_Delta || values[WY] > a_Delta) { return false; }

		if (values[TX] < -a_Delta || values[TX] > a_Delta) { return false; }
		if (values[TY] < -a_Delta || values[TY] > a_Delta) { return false; }
		if (values[CZ] - 1.f < -a_Delta || values[CZ] - 1.f > a_Delta) { return false; }

		return true;
	}

	// ================================
	// Functions
	// ================================

	TB_INLINE Mat3x3& Mat3x3::Clear()
	{
		for (int i = 0; i < 9; i++)
		{
			values[i] = 0.f;
		}

		return *this;
	}

	TB_INLINE Mat3x3& Mat3x3::Identity()
	{
		/*      */   values[Y1] = values[WX] =
		values[X2] = /*      */   values[WY] =
		values[TX] = values[TY]   /*      */ = 0.f;

		values[SX] = /*      */   /*      */
		/*      */   values[SY] = /*      */
		/*      */   /*      */   values[CZ] = 1.f;

		return *this;
	}

	TB_INLINE Mat3x3& Mat3x3::Transpose()
	{
		Mat3x3 temp = *this;

		/*      */
		values[Y1] = temp.values[X2];
		values[WX] = temp.values[TX];

		values[X2] = temp.values[Y1];
		/*      */
		values[WY] = temp.values[TY];

		values[TX] = temp.values[WX];
		values[TY] = temp.values[WY];
		/*      */

		return *this;
	}

	TB_INLINE Mat3x3 Mat3x3::GetTransposed() const
	{
		return Mat3x3(
			values[X1], values[X2], values[TX],
			values[Y1], values[Y2], values[TY], 
			values[WX], values[WY], values[CZ]
		);
	}

	TB_INLINE Mat3x3& Mat3x3::Invert()
	{
		Mat3x3 result;

		result.values[X1] = (values[Y2] * values[CZ]) - (values[TY] * values[WY]);
		result.values[Y1] = (values[TY] * values[WX]) - (values[Y1] * values[CZ]);
		result.values[WX] = (values[Y1] * values[WY]) - (values[Y2] * values[WX]);

		result.values[X2] = (values[TX] * values[WY]) - (values[X2] * values[CZ]);
		result.values[Y2] = (values[X1] * values[CZ]) - (values[TX] * values[WX]);
		result.values[WY] = (values[X2] * values[WX]) - (values[X1] * values[WY]);

		result.values[TX] = (values[X2] * values[TY]) - (values[TX] * values[Y2]);
		result.values[TY] = (values[TX] * values[Y1]) - (values[X1] * values[TY]);
		result.values[CZ] = (values[X1] * values[Y2]) - (values[X2] * values[Y1]);

		float r = (values[X1] * result.values[X1]) + (values[Y1] * result.values[X2]) + (values[WX] * result.values[TX]);
		float rcp_r = 1.f / r;
		float abs_r = Math::Abs(r);

		result.values[X1] *= rcp_r;
		result.values[Y1] *= rcp_r;
		result.values[WX] *= rcp_r;

		result.values[X2] *= rcp_r;
		result.values[Y2] *= rcp_r;
		result.values[WY] *= rcp_r;

		result.values[TX] *= rcp_r;
		result.values[TY] *= rcp_r;
		result.values[CZ] *= rcp_r;

		*this = result;

		return *this;
	}

	TB_INLINE Mat3x3 Mat3x3::GetInverted() const
	{
		Mat3x3 result;

		result.values[X1] = (values[Y2] * values[CZ]) - (values[TY] * values[WY]);
		result.values[Y1] = (values[TY] * values[WX]) - (values[Y1] * values[CZ]);
		result.values[WX] = (values[Y1] * values[WY]) - (values[Y2] * values[WX]);

		result.values[X2] = (values[TX] * values[WY]) - (values[X2] * values[CZ]);
		result.values[Y2] = (values[X1] * values[CZ]) - (values[TX] * values[WX]);
		result.values[WY] = (values[X2] * values[WX]) - (values[X1] * values[WY]);

		result.values[TX] = (values[X2] * values[TY]) - (values[TX] * values[Y2]);
		result.values[TY] = (values[TX] * values[Y1]) - (values[X1] * values[TY]);
		result.values[CZ] = (values[X1] * values[Y2]) - (values[X2] * values[Y1]);

		float r = (values[X1] * result.values[X1]) + (values[Y1] * result.values[X2]) + (values[WX] * result.values[TX]);
		float rcp_r = 1.f / r;
		float abs_r = Math::Abs(r);

		result.values[X1] *= rcp_r;
		result.values[Y1] *= rcp_r;
		result.values[WX] *= rcp_r;

		result.values[X2] *= rcp_r;
		result.values[Y2] *= rcp_r;
		result.values[WY] *= rcp_r;

		result.values[TX] *= rcp_r;
		result.values[TY] *= rcp_r;
		result.values[CZ] *= rcp_r;

		return result;
	}

	TB_INLINE Vec2 Mat3x3::Transform(const Vec2& a_Position)
	{
		Vec2 result;

		float x = (a_Position.x * values[X1]) + (a_Position.y * values[X2]) + values[TX];
		float y = (a_Position.x * values[Y1]) + (a_Position.y * values[Y2]) + values[TY];
		float z = (a_Position.x * values[WX]) + (a_Position.y * values[WY]) + values[CZ];

		result.x = x / z;
		result.y = y / z;

		return result;
	}

	TB_INLINE Vec2 Mat3x3::TransformRotation(const Vec2& a_Position)
	{
		return Vec2(
			(a_Position.x * values[X1]) + (a_Position.y * values[X2]),
			(a_Position.x * values[Y1]) + (a_Position.y * values[Y2])
		);
	}

	// ================================
	// Rotation
	// ================================

	TB_INLINE float Mat3x3::GetRotation() const
	{
		float sy = Math::InvSqrt((values[X2] * values[X2]) + (values[Y2] * values[Y2]));

		return Math::RadToDeg(Math::ArcTan2(values[X2] * sy, values[Y2] * sy));
	}

	TB_INLINE Mat3x3& Mat3x3::AddRotation(float a_Degrees)
	{
		/*float rads = Math::DegToRad(a_Degrees);
		const float cosAngle = Math::Cos(rads);
		const float sinAngle = Math::Sin(rads);

		float x, y;

		x = values[X1];
		y = values[Y1];

		values[X1] = (x * -sinAngle) + (y *  cosAngle); 
		values[Y1] = (x *  cosAngle) + (y *  sinAngle);

		x = values[X2];
		y = values[Y2];

		values[X2] = (x * -sinAngle) + (y *  cosAngle); 
		values[Y2] = (x *  cosAngle) + (y *  sinAngle);*/

		/*float x1 = values[X1];
		float y1 = values[Y1];

		float x2 = values[X2];
		float y2 = values[Y2];

		values[X1] = (x2 * -sinAngle) + (y2 *  cosAngle); 
		values[Y1] = (x2 *  cosAngle) + (y2 *  sinAngle);

		values[X2] = (x1 * -sinAngle) + (y1 *  cosAngle); 
		values[Y2] = (x1 *  cosAngle) + (y1 *  sinAngle);*/

		float rads = Math::DegToRad(a_Degrees);
		const float cosAngle = Math::Cos(rads);
		const float sinAngle = Math::Sin(rads);

		float x1 = values[X1];
		float y1 = values[Y1];

		float x2 = values[X2];
		float y2 = values[Y2];

		values[X1] = ( cosAngle * x1) + ( sinAngle * x2);
		values[Y1] = ( cosAngle * y1) + ( sinAngle * y2);

		values[X2] = (-sinAngle * x1) + ( cosAngle * x2);
		values[Y2] = (-sinAngle * y1) + ( cosAngle * y2);

		return *this;
	}

	TB_INLINE Mat3x3& Mat3x3::SetRotation(float a_Degrees)
	{
		float rads = Math::DegToRad(a_Degrees);
		const float cosAngle = Math::Cos(rads);
		const float sinAngle = Math::Sin(rads);

		values[X1] =  cosAngle; values[Y1] =  sinAngle;
		values[X2] = -sinAngle; values[Y2] =  cosAngle;

		return *this;
	}

	// ================================
	// Translation
	// ================================

	TB_INLINE Vec2 Mat3x3::GetTranslation() const
	{
		return Vec2(values[TX], values[TY]);
	}

	TB_INLINE Mat3x3& Mat3x3::AddTranslation(const Vec2& a_Translation)
	{
		values[TX] += a_Translation.x;
		values[TY] += a_Translation.y;

		return *this;
	}

	TB_INLINE Mat3x3& Mat3x3::AddTranslation(float a_X, float a_Y)
	{
		values[TX] += a_X;
		values[TY] += a_Y;

		return *this;
	}

	TB_INLINE Mat3x3& Mat3x3::SetTranslation(const Vec2& a_Translation)
	{
		values[TX] = a_Translation.x;
		values[TY] = a_Translation.y;

		return *this;
	}

	TB_INLINE Mat3x3& Mat3x3::SetTranslation(float a_X, float a_Y)
	{
		values[TX] = a_X;
		values[TY] = a_Y;

		return *this;
	}

	TB_INLINE Mat3x3& Mat3x3::SetTranslationTransformed(const Vec2& a_Translation)
	{
		float x =         (a_Translation.x * values[X1]) + (a_Translation.y * values[X2]) + values[TX];
		float y =         (a_Translation.x * values[Y1]) + (a_Translation.y * values[Y2]) + values[TY];
		float rz = 1.f / ((a_Translation.x * values[WX]) + (a_Translation.y * values[WY]) + values[CZ]);

		values[TX] = x * rz;
		values[TY] = y * rz;

		return *this;
	}

	TB_INLINE Mat3x3& Mat3x3::SetTranslationTransformed(float a_X, float a_Y)
	{
		float x =         (a_X * values[X1]) + (a_Y * values[X2]) + values[TX];
		float y =         (a_X * values[Y1]) + (a_Y * values[Y2]) + values[TY];
		float rz = 1.f / ((a_X * values[WX]) + (a_Y * values[WY]) + values[CZ]);

		values[TX] = x * rz;
		values[TY] = y * rz;

		return *this;
	}

	// ================================
	// Scale
	// ================================

	TB_INLINE Vec2 Mat3x3::GetScale() const
	{
		return Vec2(
			Math::Sqrt((values[X1] * values[X1]) + (values[Y1] * values[Y1])),
			Math::Sqrt((values[X2] * values[X2]) + (values[Y2] * values[Y2]))
		);
	}

	TB_INLINE Mat3x3& Mat3x3::AddScale(const Vec2& a_Scale)
	{
		values[X1] *= a_Scale.x;
		values[Y1] *= a_Scale.x;

		values[X2] *= a_Scale.y;
		values[Y2] *= a_Scale.y;

		return *this;
	}

	TB_INLINE Mat3x3& Mat3x3::AddScale(float a_Scale)
	{
		values[X1] *= a_Scale;
		values[Y1] *= a_Scale;

		values[X2] *= a_Scale;
		values[Y2] *= a_Scale;

		return *this;
	}

	TB_INLINE Mat3x3& Mat3x3::SetScale(const Vec2& a_Scale)
	{
		float sx = Math::InvSqrt((values[X1] * values[X1]) + (values[Y1] * values[Y1]));
		float sy = Math::InvSqrt((values[X2] * values[X2]) + (values[Y2] * values[Y2]));

		values[X1] *= sx * a_Scale.x;
		values[Y1] *= sx * a_Scale.x;

		values[X2] *= sy * a_Scale.y;
		values[Y2] *= sy * a_Scale.y;

		return *this;
	}

	TB_INLINE Mat3x3& Mat3x3::SetScale(float a_Scale)
	{
		float sx = Math::InvSqrt((values[X1] * values[X1]) + (values[Y1] * values[Y1]));
		float sy = Math::InvSqrt((values[X2] * values[X2]) + (values[Y2] * values[Y2]));

		values[X1] *= sx * a_Scale;
		values[Y1] *= sx * a_Scale;

		values[X2] *= sy * a_Scale;
		values[Y2] *= sy * a_Scale;

		return *this;
	}

}
